Spatial avoidance method and apparatus

ABSTRACT

An obstacle avoidance system, apparatus, and method is described suitable for use on autonomously guided or man-in-the-loop guided vehicles such as aircraft, missiles, cars, and other types of vehicles. The system provides guidance instructions in the situation where the vehicle encounters an obstacle either directly or indirectly in its path while traveling to a desired destination or where vehicles are traveling in formation. The system can be applied not only to obstacle avoidance but also to trajectory shaping by defining obstacles, operational boundaries and/or threats which influence the trajectory of the vehicle.

RELATED APPLICATIONS

This application claims the benefit of U.S. Provisional PatentApplication Ser. No. 60/171,314, filed on Dec. 21, 1999, the entirecontents of which are hereby incorporated by reference.

FIELD OF THE INVENTION

The present invention relates to a guidance method and system suitablefor use with autonomously guided and man-in-the-loop guided vehicleswhere the presence of obstacles and/or threats must be considered inguiding the vehicle to a destination.

BACKGROUND OF THE INVENTION

During the travel of an autonomously guided or man-in-the-loop guidedvehicle towards a destination, there may be obstacles, impedimentsand/or threats present in the path of travel which must be avoided whichotherwise would prevent the vehicle from reaching its destination orwould cause damage to the vehicle. Various guidance augmentationconcepts have been suggested for avoiding obstacles, operationalboundaries, and/or threats. Generally, these concepts have beendeveloped with the approach such that if no impediments are present,then the travel of the guided vehicle is not changed. However, ifimpediments are present, these concepts provide the guidance system withdirections diverting the vehicle around the impediment to allow thevehicle to proceed to the desired destination.

These avoidance methods have been used, for instance, in the field oftactical missiles. There, during the launch, flight, and interceptphases of flight, there are numerous constraints on the guidance systemthat result from obstacles or operational limitations that must beaccommodated in addition to the requirement that the missile be guidedso as to hit the target. Some of these constraints includemaneuverability constraints based on dynamic, kinematic, and/ormechanical constraints of the vehicle; operational envelope constraintssuch as maximum altitude or minimum altitude limitations on the vehicle;obstacle constraints, such as friendly airborne assets; and, threatconstraints such as enemy assets which may target the vehicle in anattempt to destroy the vehicle. All of these factors are significantconsiderations in shaping the trajectory of the missile during itsflight. Various textbook guidance methods that have been proposed willbe briefly discussed below. These methods, however, do not take intoaccount all of those constraints and therefore would benefit fromaugmentation in some form prior to implementation in a real worldapplication.

A first known method for accommodating guidance constraints is known asa waypoint method. This guidance method uses waypoints or intermediatepoints along a trajectory. One such concept is described in the article“Obstacle-Avoidance Automatic Guidance: A Concept Development Study” byVictor H. L. Cheng, published in AIAA Journal, Paper Number 88-4189-CP,1988. The basic idea behind the waypoint concept is that if an obstacle,operational boundary, or threat is perceived, a set of waypoints isconstructed such that an avoidance path can be achieved. This concept isillustrated in FIG. 1. As shown in FIG. 1, a vehicle 10 can be navigatedfrom a starting point 110 to a destination 140 via waypoints 120 and130. To generate an efficient trajectory, the waypoints must be selectedcarefully. The waypoint method is suitable for applications where theimpediments are stationary and are not dynamic.

Another known avoidance method is to use optimal guidance strategies tosteer a vehicle around obstacles or boundaries. One such approach isdescribed in the article “Control Theoretic Approach to Air TrafficConflict Resolution”, AIAA Journal, Paper Number 93-3832-CP, 1993. Thismethod involves the definition of a cost function that measures thequality or goodness of a particular trajectory and which optimizes thecost function over a set of possible trajectories. The construction ofthe cost function is generally based on the dynamics or relative motionof the obstacles, operational boundaries, and/or threats to the missile.Generally, this method must be solved numerically and can require largeamounts of real time processing to solve. Also, the addition of multipleobstacles, operational boundaries, and/or dynamic threats in theoperation or flight significantly increases the complexity of the costfunction and thus further increases processing requirements.

Another avoidance method relies upon mathematical representations ofpotential fields. In this approach, sources, which are potential fieldelements that provide a mathematically repelling force, can be usedindividually or as a surface to provide a range dependent force in orderto push the missile guidance away from the obstacle. Sources have aunique quality in that the amount of repelling force is inverselyproportional to the distance from the source. For example, a missilethat is close to a source will be pushed away with greater force than amissile that is far from the source. Because of this range dependentcharacteristic, and because sources are computationally efficient touse, sources can be used in a large number of avoidance applications.However, source methods have a drawback that, if they are not modified,they will affect the guidance commands by directing the vehicle awayfrom the obstacle throughout the trajectory of the vehicle because thesource has an infinite range of influence.

A modification to the source approach adds a range boundary beyond whichthe source will not affect the vehicle. One such approach is describedin “Generation of Conflict Resolution Maneuvers for Air TrafficManagement”, by Claire, et al., IEEE Journal, Paper No. 0/7803-4119-8,1997. In this approach, the method will push the guidance command awayfrom the obstacle only when the missile is inside the range boundary andwill not affect the vehicle when it is outside the boundary. Onedrawback to this method is that the guidance command is mathematicallydiscontinuous across the boundary. Discontinuous commands areundesirable because they can cause instabilities in the guidancesolution in the circumstance when multiple obstacles, boundaries, orthreats are encountered. Another drawback to this method is in regard tothreat avoidance. Specifically, this method will tend to direct avehicle along the direction of the velocity of the threat in order toavoid the threat. However, if the threat has sufficient velocity toovertake the vehicle and the source potential is located on the vehicle,the threat can nevertheless have a successful intercept. The Claire, etal. article also mentions the use of vortex potential field elements inthe construction of an avoidance solution. Vortex elements areessentially planar rotation elements that push the field in a rotationaldirection about a point. Vortex elements are not easily adapted tomulti-dimensional spaces above two dimensions, because they aretwo-dimensional elements. To apply a vortex element in athree-dimensional space would require knowledge of the direction whichthe vortex would act about. In a dynamic situation, the addition of avortex element would limit the number of evasive solutions and wouldhinder the performance of an avoidance algorithm. Nevertheless,two-dimensional applications of vortex elements can be appropriate incircumstances such as traffic management applications where the flow ofvehicles conforms to pre-established guidelines.

Another known avoidance method employs specific geometric boundaryshapes to describe how an avoidance maneuver is to be performed. Thesegeometric methods use the surface tangents from the geometric boundaryto provide a direction of avoidance for the avoidance maneuver. One suchavoidance approach is described in “A Self-Organizational Approach forResolving Air Traffic Conflicts”, pages 239-254, by Martin S. Eby,published in The Lincoln Laboratory Journal, Volume 7, No. 2, 1994. Suchan approach is illustrated in FIG. 2, where a vehicle 10 is traveling indirection of arrow D and must avoid obstacle plane 200. A geometricboundary shape 210 is defined around obstacle plane 200 at a radiusr_(D) which is the desired miss distance. In the case where the path ofthe subject plane 10 to its destination 220 intersects the geometricboundary shape 210, an avoidance maneuver is necessary. In thiscircumstance, an avoidance velocity vector solution V_(S) is found, inpart, by forming a tangent 230 to the geometric boundary 210. Asignificant limitation to this method with regard to tactical missilesis that if the missile gets inside of the avoidance geometric boundary,then the tangent vector to the surface no longer exists. The Claire, etal. article also discusses how the size of the geometric boundary shapecan be reduced in order to allow for calculation of a tangent.Specifically, in the case where the missile gets inside of the avoidancegeometric boundary, the geometric boundary shape is reduced until themissile is returned to the exterior of the geometric boundary. Thereduction of the size of the geometric boundary can work for obstacleavoidance because as a subject vehicle approaches the surface geometry,the guidance direction becomes more aggressive in an avoidancedirection.

One shortcoming of the method occurs when trying to provide anoperational boundary such as a maximum altitude for a vehicle. Onerepresentation of an absolute altitude ceiling is a single sphere whichsurrounds the earth at a specified altitude. Such a sphere, however,would immediately cause a problem with the above-described methodbecause the vehicle would always be inside the geometric boundary andthus no tangent could be formed. Such a method would also havedifficulty with an altitude ceiling limitation if a maximum altitudewere represented by a plane. In this case, the geometric tangent methodwould provide an avoidance command that was always parallel to theplane. As a result, the desired avoidance command would be ignored atall times.

The tangent geometric boundary avoidance technique is also inadequate inthe circumstance of launching a missile from an aircraft. Specifically,the tangent method cannot accommodate the situation where the missile isinside the geometric shape which surrounds the vehicle to be avoided.Thus, the geometric shape must be made very small. However, if thegeometric shape surrounding a launching aircraft is made small enough toallow the tangent method to work, the geometric shape would be smallerthan the aircraft to be avoided, no avoidance commands would be issued,and the missile would be able to fly into the launching aircraft.

Accordingly, in view of the above, what is needed is an improvedavoidance system and method which can address the shortcomings outlinedabove.

SUMMARY OF THE INVENTION

An object of the present invention is to provide an avoidance system andmethod which addresses both static and dynamic obstacles and which doesnot require specialized algorithms for dynamic obstacles.

Another object of the present invention is to provide an obstacleavoidance system and method which provide a deterministic avoidancecommand and which does not require an iterative minimization algorithmfor a solution.

A further object of the present invention is an avoidance system andmethod which comprehends the situation where the vehicle is inside of ageometric boundary shape.

According to the present invention, the foregoing objects are attainedby blending together a distance-based field element (such as source,sink, or vortex), a geometric boundary surface normal and tangent, anoriginal guidance command, and the distance to the obstacle to producean avoidance command that will direct the missile away from the obstacleand proceed in the general direction of the target destination.

The avoidance commands are constructed in a fashion such that, in thecase where a vehicle is very close to an obstacle, the avoidance commanddirects the vehicle in a direction to prevent collision. When a vehicleapproaches a geometric boundary, the avoidance command is blended froman anti-intercept direction to a direction that is a function of asurface tangent, such that the avoidance command is continuous acrossthe geometric boundary. Outside the geometric boundary, the guidancecommand is blended with a vector that is a function of the surfacetangent taken in a distance dependent fashion to provide an avoidancedirection along the desired direction of travel. As a result, theinclusion of obstacles and/or threats in the operational space of avehicle will produce an alteration to the nominal guidance commands yetstill allow the vehicle to travel to its intended final destination orfinal state. In the absence of obstacles, operational boundaries orthreats, the present invention will not alter the original guidancecommands and the vehicle will travel according to the original guidanceimplementation.

This invention may be applied in applications such as autonomousvehicles as well as vehicles employing man-in-the-loop guidance.Specifically, such as in the situation of a flight director, an operatorof the vehicle may be informed of a proper guidance adjustment requiredto avoid an obstacle.

BRIEF DESCRIPTION OF THE DRAWINGS

A more complete understanding of the present invention and itsadvantages will be readily apparent from the following DetailedDescription taken in conjunction with the accompanying drawings.Throughout the accompanying drawings, like parts are designated by likereference numbers.

FIG. 1 shows an example of a vehicle being maneuvered along a flightpath by the method of waypoints as known in the prior art;

FIG. 2 shows a geometric boundary avoidance method of the prior art;

FIG. 3A shows a first embodiment where an avoidance command isdetermined in a case where the object is contained inside a geometricboundary and where the vehicle is outside the geometric boundary;

FIG. 3B shows a detailed view of the vector analysis used in a firstembodiment where an avoidance command is determined in a case where theobject is contained inside a geometric boundary and where the vehicle isoutside of the geometric boundary;

FIG. 3C shows a detailed view of the vector analysis used in a firstembodiment where an avoidance command is determined in a case where theobject is contained inside a geometric boundary and where the vehicle isoutside of the geometric boundary and has passed the object;

FIG. 4 shows a first embodiment where an avoidance command is determinedin the case where the object is contained inside a geometric boundaryand where the vehicle is also inside the geometric boundary;

FIG. 5 shows an operational boundary, such as a minimum altitude, toprevent the vehicle from flying below a minimum altitude boundary limit;

FIG. 6 shows an operational boundary, such as a maximum altitude, toprevent the vehicle from climbing above the boundary limit;

FIG. 7 shows an operational boundary to prevent a vehicle, such as asubmarine, from entering a specified boundary area;

FIG. 8 shows a vehicle avoidance process using the present invention inthe situation where the vehicle is being pursued by a threat;

FIG. 9A illustrates a missile launch from an aircraft without using thepresent invention to prevent the missile from impacting the launchingaircraft;

FIG. 9B shows an application of a missile launch from an aircraft usingthe present invention to prevent the missile from impacting thelaunching aircraft;

FIGS. 10A and 10B show an application of the present invention tocontrol vehicles traveling together in formation;

FIGS. 10C through 10E show another application of the present inventionto control vehicles traveling together in formation;

FIG. 11 is a vector field diagram representing a situation where onlyone obstacle is present;

FIG. 12 is a vector field diagram representing a situation wheremultiple obstacles are present in the vector field;

FIG. 13 is a vector field diagram representing the presence of multipleobstacles in the vector field with intersecting obstacle boundaries;

FIG. 14 is a vector field diagram representing the presence of multipleobstacles having finite size with intersecting obstacle boundaries;

FIG. 15 is a vector field diagram representing the presence of multipleobstacles having finite size with intersecting obstacle boundaries wherethe destination is inside a geometric boundary;

FIG. 16 is a vector field diagram representing the presence of multipleobstacles having finite size with intersecting obstacle boundaries andthe presence of a non-closed infinite boundary surface;

FIG. 17 is a vector field diagram representing the presence of multipleobstacles having finite size with intersecting obstacle boundaries, thepresence of a non-closed infinite boundary surface, and with tangentboundary direction control;

FIG. 18 is a vector field diagram representing the presence of multipleobstacles having finite size with intersecting obstacle boundaries, thepresence of a non-closed infinite boundary surface with tangent boundarydirection control, and with multiple potential types;

FIG. 19 is a vector field diagram illustrating application of thepresent invention for use as an automated flow manager in trafficcorridor;

FIG. 20 is a vector field diagram illustrating an application of thepresent invention in a Six-Degree-of-Freedom (6DOF) vehicle controlapplication;

FIG. 21 shows construction of the components for the 6DOF guidancemethod;

FIG. 22 is a block diagram of an autonomous vehicle guidance processingunit;

FIG. 23 is a block diagram of an automatic guidance processing unit fora piloted vehicle; and

FIG. 24 is a block diagram of a pilot controlled guidance processingunit for a piloted vehicle.

DETAILED DESCRIPTION OF THE PREFERRED EMBODIMENTS

Preferred Embodiments of the present invention will now be described indetail in reference to the various drawings.

As noted above, an avoidance method and system in accordance with thepresent invention combine the ability to avoid obstacles while allowingthe vehicle to proceed to a final desired destination. In the case of atactical missile, the primary mission of the missile is to fly to atarget destination, which may be in motion or at rest, and to deliver atthe destination a payload. Along the trajectory, the missile mayencounter obstacles or impediments, which may be in the form of friendlyairborne assets, operational boundaries which may define altitudeceilings and floors, and threats, which may try to intercept the missileduring its mission. The objective of the avoidance method and system isto provide a guidance command or direction (e_(avoid)) which can directthe missile so as to avoid the obstacles and still permit the vehicle toreach the intended target destination.

In order to operate, the present method and system require informationabout the state of each obstacle. Such information may include theobstacle's position, velocity and heading, acceleration, or some otherquantity which can be used in the avoidance method computation. Themethod is not dependent on how the information is obtained, however, theeffectiveness of the resulting obstacle avoidance commands which resultfrom the present method will be influenced by the accuracy of the stateinformation provided with regard to the obstacle(s).

First Embodiment Obstacle Avoidance

A First Embodiment of the present invention is described in regard to animplementation where an obstacle is contained inside a geometricboundary.

Vehicle Outside a Geometric Boundary of the Object

A First Embodiment of the present invention involves implementation ofthe obstacle avoidance method computation where the vehicle to bemaneuvered to avoid the obstacle is outside a geometric boundary of theobstacle as shown in FIG. 3A. The geometric boundary includes theposition of the obstacle and a region surrounding the obstacle. FIG. 3Agenerally illustrates the situation where a vehicle 10 is approaching anobject to be avoided 300, where the object to be avoided 300 iscontained inside a geometric boundary 310 and where the vehicle 10 isoutside the geometric boundary 310. Various guidance vectors “e” areshown in FIG. 3A, and will be individually described in more specificdetail below.

It should be noted that in the following discussion, when the boundarydefines an object or region to be avoided, the object and itssurrounding region are referred to as a source element. When theboundary defines an object or region towards which the vehicle isattracted, the object or region is referred to as a sink element. Forthe purpose of understanding the present method, however, the repellingeffect of a source or the attracting effect of a sink are differentprincipally in the sign (i.e., + or −) of the forces and vectorsinvolved. The fundamental principles described next are otherwise thesame for sources and sinks.

As illustrated in FIG. 3A, vehicle 10 travels at velocity V_(p)following a nominal guidance command e_(desired) which represents theguidance command that vehicle 10 would be commanded to follow in theabsence of any obstacle whose presence would cause a deviation. Obstacle300 may be moving at a velocity V_(imp). However, for purposes ofdiscussion at this time, V_(imp) is substantially zero. Vector e_(int)represents a line of intercept between the obstacle 300 and the vehicle10. Point R represents the point of intersection of vector e_(int) onthe surface of the geometric boundary 310. Plane 320 is a plane which istangent to the geometric boundary 310 at point R, and point R is thesurface point of tangency of plane 320. Vector e_(N) is a surface normalvector at the surface point of tangency, R. Vector e_(tan) is a vectorrepresenting a component of the nominal guidance command e_(desired)projected onto tangent plane 320 and is the component in the directionof the nominal guidance command e_(desired). As will be explained morefully below, plane 330 represents a transition zone between a statewhere the vehicle 10 is approaching the obstacle 300 and a state wherethe vehicle 10 has overtaken and is receding from the obstacle 300.Vector e′_(tan) is the geometric based avoidance vector which isdetermined from e_(N) and e_(tan). Vector e_(avoid) represents theresulting avoidance guidance command, the computation of which will beexplained below in reference to FIG. 3B.

FIG. 3B more specifically illustrates the applicable vectors used indetermining the resulting avoidance guidance command e_(avoid) in asituation similar to that shown in FIG. 3A, where an object to beavoided is contained inside a geometric boundary and the vehicle isoutside the geometric boundary. For discussion purposes, all vectors inFIG. 3B are shown originating at R to demonstrate the relativerelationship between the vectors.

As illustrated in FIG. 3B, a plane 320 tangent to a geometric surface ofthe geometric boundary 310 is defined having its point of tangency atpoint R, which is the intersection of the geometric surface of thegeometric boundary with the vector e_(int) defined by the relativeposition of a vehicle (not shown but positioned as if at R) to theobstacle 300. Vector e_(tan) is a component of the nominal guidancecommand, e_(desired), when e_(desired) is projected onto plane 320 sothat e_(tan) is in the direction of the nominal guidance command.

As shown in equations (1a) and (1b), the surface normal e_(N) and thetangent direction at the surface point of tangency e_(tan) are thencombined through the use of linear or non-linear functions h₁(x) andh₂(x) to provide a geometric-based avoidance vector e′_(tan). Thisapproach blends vector e_(N) with vector e_(tan) such that the geometricavoidance vector e′_(tan) can range from e_(tan) to e_(N) depending onthe value of functions h₁ (x) and h₂(x). According to the FirstEmbodiment,

e′ _(tan) =E′ _(tan) /∥E′ _(tan)∥  (1a)

where

E′ _(tan) =h ₁(x)e _(N) +h ₂(x)e _(tan)  (1b)

where 0≦h₁(x)≦∞ and 0≦h₂(x)≦∞, such that h₁(x) and h₂(x) are not bothzero. For all equations of the form above, functions such as h₁(x) andh₂(x) should be constructed such that ∥E′_(tan)∥≠0.

One particular example of functions h₁(x) and h₂(x) which satisfies theabove criteria is where h₂(x) is defined as 1−h₁(x), where 0≦h₁(x)≦1.However, in the obstacle avoidance method of the present invention,functions h₁(x) and h₂(x) need not be related. Any other avoidancefunctions may alternately be used in the computation so long as theysatisfy the above requirements.

Referring to FIGS. 3A and 3B, after geometric avoidance vector e′_(tan)is determined, the resulting avoidance guidance command e_(avoid) isdetermined, as shown in equations (2a) and (2b), by blending togethere′_(tan) with e_(desired), based on functions f₁(x) and f₂(x), such thate_(avoid) can range from e_(desired) to e′_(tan) depending on the valueof functions f₁(x) and f₂(x). According to the First Embodiment,

e _(avoid) =E _(avoid) /∥E _(avoid)∥  (2a)

where

E _(avoid) =f ₁(x)e _(desired) +f ₂(x)e′ _(tan)  (2b)

where 0≦f₁(x)≦∞ and 0≦f₂(x)≦∞, such that f₁(x) and f₂(x) are not bothzero; and where f₂(x)=0, when the vehicle is at or beyond point L, pointL being a point on the influence surface 309 located a distance from theobstacle 300 to be avoided along e_(int); and f₁(x)=0, when the vehicle10 is at point R. Further, functions f₁(x) and f₂(x) should beconstructed such that ∥E_(avoid)∥≠0.

One particular example of functions f₁(x) and f₂(x) is where f₂(x) isdefined as 1−f₁(x), where 0≦f₁(x)≦1. However, in the obstacle avoidancemethod of the present invention, functions f₁(x) and f₂(x) need not berelated. Other avoidance functions may alternately be used in thecomputation.

As noted above for FIG. 3A, plane 330 represents a transition zone forchanging the avoidance command as the vehicle 10 overtakes and passesthe obstacle 300. More specifically, when the vehicle is outside thegeometric boundary 310 of the obstacle 300, if the avoidance guidancecommand e_(avoid), as described and determined above, is applied as thevehicle 10 overtakes and passes the obstacle 300, the guidance commandgenerated will unnecessarily and undesirably follow the e_(tan) vectorafter passing the obstacle 300.

Referring now to FIG. 3C, once the vehicle 10 has passed the obstacle300, the e′_(tan) vector can be blended with the e_(desired) vector byusing continuous linear or non-linear functions g₁(x) and g₂(x) toconstruct e_(tan)*. The vector e_(tan)* is constructed based on e′_(tan)and e_(desired) through functions g₁(x) and g₂(x) so thate_(tan)*→e′_(tan) when the vehicle 10 is approaching the obstacle 300and e_(tan)*→e_(desired) after the vehicle 10 passes the obstacle 300while remaining outside the geometric boundary 310. Like functions,h₁(x) and h₂(x), functions g₁(x) and g₂(x) may be limited to operationover a specified range, γ.

Vector e_(tan)* can be described in accordance with functions (3a), (3b)and as follows:

e _(tan) *=E _(tan) */∥E _(tan)*∥  (3a)

where

E _(tan) *=g ₁(x)e _(desired) +g ₂(x)e′ _(tan)  (3b)

where 0≦g₁(x)≦∞ and 0≦g₂(x)≦∞, such that g₁(x) and g₂(x) are not bothzero; and g₂(x)=0, when the vehicle 10 is at an end 306 of thetransition range γ, and g₁(x)=0 when the vehicle 10 is at a beginning307 of the transition range γ. Further, functions g₁(x) and g₂(x) shouldbe constructed such that ∥E_(tan)∥≠0.

In an exemplary embodiment, g₁(x)=g₁(x, γ) and g₂(x)=g₂(x, γ) whereg₂(x, γ)=1−g₁(x, γ) and where γ is a continuous multi-variable functionwhich describes the spatial boundary over which the command will betransitioned from e_(desired) to e′_(tan).

Thus, in equations above, when functions g₁(x) and g₂(x) are used tomodify the behavior of the avoidance command as the vehicle 10 passesthe obstacle 300, e_(tan)* may then be substituted into equation (2b) toyield equation (4b), shown below, which is a more robust solution forobtaining e_(desired) and which addresses the desirable transition ofthe guidance command after the vehicle 10 passes the obstacle 300.

E _(avoid) =f ₁(x)e _(desired) +f ₂(x)e _(tan)*  (4b)

where f₁(x) and f₂(x) are as defined above in regard to equations (2a)and (2b), and where e_(tan)* is determined as defined above in regard toequations (3a) and (3b).

As was discussed above in previous computations of the method, e_(avoid)is computed by the following equation:

e _(avoid) =E _(avoid) /∥E _(avoid)∥  (4a).

It will be appreciated by those skilled in the art that all functionsdescribed herein may be either linear or non-linear continuousfunctions. Also, for simplicity, the parameter, x, is used as thevariable parameter of all functions throughout the discussion of theobstacle avoidance method computation. The variable “x” may represent aparameterized constant or set of constants, varying function, or familyof functions. For example only and not by way of limitation, “x” mayrepresent position, distance, time, angle, and/or acceleration. It willbe appreciated by those skilled in the art that “x” may representmultiple functions or a multi-variable function, e.g. d(x) may insteadbe d(a, b) or d(a, b, γ). It will further be appreciated by thoseskilled in the art that “x” need not represent the same parameter or setof parameters in every function of an equation, nor need it representidentical variables in functions of different equations of the obstacleavoidance method computation.

Both Vehicle and Object are Inside a Geometric Boundary

The situation discussed above relative to FIGS. 3A, 3B and 3C addressesthe situation where the object to be avoided 300 is contained inside ofa geometric boundary 310 and where the approaching or retreating vehicle10 is outside the geometric boundary 310. As will be discussed next withregard to FIG. 4, the First Embodiment of the present invention can alsoeasily address the situation where both the vehicle 10 and the object tobe avoided 300 are inside of a geometric boundary 310

FIG. 4 generally illustrates the situation where a vehicle 10 isapproaching an object to be avoided 300, where both the object to beavoided 300 and the vehicle 10 are contained inside the geometricspatial boundary 310. Various guidance vectors “e” shown in FIG. 4generally correspond to the guidance vectors shown in FIGS. 3A and 3Band will be more specifically described below.

Vehicle 10 travels at velocity V_(p) following a nominal guidancecommand e_(desired) which represents the guidance command that vehicle10 would be commanded to follow in the absence of any obstacle whosepresence would cause a deviation. Vector V_(imp) represents the velocityof the obstacle 300 and is considered for illustrative purposes only tobe zero. Vector e_(int) represents a line of intercept between thevehicle 10 and the obstacle 300. Point R represents the point ofintersection of vector e_(int) on the surface of the geometric boundary310. Plane 320 is a plane which is tangent to the geometric boundary 310at the surface point of tangency R. Vector e_(N) is a surface normalvector at the surface point of tangency R. Vector e_(tan) is a vectorrepresenting the projection of the nominal guidance command e_(desired)onto tangent plane 320 and is in the direction of the nominal guidancecommand e_(desired). As shown in FIGS. 3A and 4, plane 330 represents atransition zone between a state where the vehicle 10 is approaching theobstacle 300 and a state where the vehicle 10 has overtaken and isreceding from the obstacle 300. Vector e′_(tan) is the geometric basedavoidance vector which is determined from e_(N) and e_(tan). Vectore_(avoid) represents the resulting avoidance guidance command, thecomputation of which will be explained below.

In the case where the vehicle 10 is inside the geometric boundary 310(FIG. 4), as with the case where the vehicle 10 is outside the geometricboundary surface 310 (FIG. 3A), a geometric-based avoidance vectore′_(tan) is determined in accordance with equations (1a) and (1b) byblending the surface normal e_(N) and the tangent direction at thesurface point of tangency e_(tan) through the use of the functions h₁(x)and h₂(x). As noted above, this approach blends vector e_(N) with vectore_(tan) such that the geometric avoidance vector e′_(tan) can range frome_(tan) to e_(N) depending on the value of functions h₁(x) and h₂(x). Toavoid discontinuity in the calculation at the surface of the geometricboundary for the case where the vehicle 10 is inside the geometricboundary 310, it is preferable that h₁(x) and h₂(x) be the samefunctions as h₁(x) and h₂(x) in the case where the vehicle 10 is outsidethe geometric boundary 310. If the discontinuity is large enough, it mayproduce an unstable guidance mode whereby the guidance command willoscillate about a surface of the geometric boundary 310 and not producethe desired avoidance effect.

After the geometric avoidance vector e′_(tan) is determined, theresulting avoidance guidance command e_(avoid) is determined. Unlike theapproach shown in equations (2a) and (2b), which are applied in thesituation where the vehicle 10 is outside the geometric boundary 310, inthe case where the vehicle 10 is inside of the geometric boundary 310,e_(avoid) is determined based on e′_(tan) and e_(int). Morespecifically, as shown in equations (5a) and (5b), e_(avoid) isdetermined by blending together e′_(tan) with e_(int), based onfunctions f′₁(x) and f′₂(x), such that e_(avoid) can range from e_(int)to e′_(tan) depending on the value of the functions f′₁(x) and f′₂(x).In this situation,

e _(avoid) =E _(avoid) /∥E _(avoid)∥  (5a)

where

E _(avoid) =f′ ₁(x)e _(int) +f′ ₂(x)e′ _(tan)  (5b)

where 0≦f′₁(x)≦∞, 0≦f′₂(x)≦∞, such that f′₁(x) and f′₂(x) are not bothzero; f′₂(x)=0, when the vehicle 10 is coincident with a surface of theobstacle 300, and where f′₁(x)=0, when the vehicle 10 is at point R. Aswith the previous example, both f′₁(x) and f′₂(x) are linear ornon-linear continuous functions with x being a parameterized constant,function, or family of functions used in the description of functionsf′₁(x) and f′₂(x), where D is the local distance from the obstacle orpoint to be avoided 300 along e_(int) to the geometric surface point oftangency R. Further, functions f′₁(x) and f′₂(x) should be constructedsuch that ∥E_(avoid)∥≠0.

Although not shown, after vehicle 10, which is inside geometric boundary310, passes obstacle 300, which is also inside geometric boundary 310,the guidance command can be modified in a manner similar to that ofeqns. (3A) and (3B). Once the vehicle 10 has passed the obstacle 300,the e′_(tan) vector can be blended with the e_(deaired) vector bycontinuous linear or non-linear functions g₁(x) and g₂(x) to constructe_(tan)*. The vector e_(tan)* is constructed based on e′_(tan) ande_(desired) through function g(x) so that e_(tan)*→e′_(tan) when thevehicle 10 is approaching the obstacle 300 and e_(tan)*→e_(desired)after the vehicle passes the obstacle, and where e_(tan)* can bedescribed in accordance with eqns. (3a) and (3b). Like functions h₁(x)and h₂(x), functions g₁(x) and g₂(x) may be limited to operation over aspecified range, γ. As with the functions, h₁(x) and h₂(x), describedabove, it is preferable that functions g₁(x) and g₂(x) be the same asg₁(x) and g₂(x) for the case where the vehicle is outside the geometricboundary in order to avoid potential discontinuity at the geometricboundary surface.

When the functions g₁(x) and g₂(x) are used to modify the behavior ofthe avoidance command as the vehicle 10, inside the geometric boundary310, passes the obstacle 300, e_(tan)* may be substituted into equation(5b) to yield equations (6a) and (6b), shown below, which is a morerobust solution for obtaining e_(avoid) and which addresses thedesirable transition of the guidance command after the vehicle 10 passesthe obstacle 300. In the above situation:

e _(avoid) =E _(avoid) /∥E _(avoid)∥  (6a)

where

E _(avoid) =f′ ₁(x)e _(desired) +f′ ₂(x)e _(tan)*  (6b)

where f′₁(x) and f′₂(x) are as defined above in regard to equations (5a)and (5b), and where e_(tan)* is determined as defined above in regard toequations (3a) and (3b).

To reiterate, during application of the spatial avoidance method, as thevehicle 10 is approached by, or approaches an obstacle and is outside ofthe geometric boundary, the direction e_(desired) will be modified basedon a function of the tangent of the geometric boundary (eqns. 1a, 1b, 2aand 2b) and based on a function of the approaching or retreatingobstacle state (eqns. 3a, 3b, 4a and 4b). Thus, at the boundary surface,the vehicle will be directed to fly tangent to the boundary andconsequently fly around the obstacle along the edge of the boundarysurface. When the vehicle has sufficiently avoided the obstacle, asdetermined by eqns. (3a) and (3b), the avoidance command is transitionedfrom an avoidance trajectory to the desired trajectory.

However, if the vehicle 10 penetrates the boundary, the directione_(desired) will be modified based on a function of the tangent of thegeometric boundary (eqns. 1a, 1b, 5a and 5b), and based on a function ofthe approaching or retreating obstacle state (eqns. 3a, 3b, 6a and 6b),such that the vehicle will be directed away from the obstacle. Themethod is continuous at the boundary surface because when the vehicle isjust inside of the boundary, the vehicle is directed to fly along atangent of the boundary, therefore providing a continuous transitionthrough the boundary. The avoidance direction is transitioned from theboundary tangent, to a direction away from the boundary tangent, basedon (for example) the distance γ from the obstacle (eqns. 1a, 1b, 5a and5b). When the vehicle has sufficiently avoided the obstacle, theavoidance command is transitioned from an avoidance trajectory to thedesired trajectory (eqns. 3a, 3b, 6a and 6b).

These concepts of avoidance are not limited to the specific geometricspaces (i.e., 3-dimensional space) as described previously. Theseavoidance concepts may be used to avoid acceleration, velocity, or othermathematically defined obstacles in N dimensional space. Theimplementation methodology will be the same. That is to say, theapplication of eqns. 1 through 6 would be the same, although theweighting functions associated with those equations would become morecomplicated. An implementation for such an algorithm can be in theavoidance of guidance algorithm singularities, whereby the resultingguidance command would be the zero vector. This construction can involvethe position, velocity and the acceleration of the vehicle as well asthe position, velocity and acceleration of the target.

For instance, one of ordinary skill in the art will appreciate that amathematically defined obstacle to be avoided can include a singularityin a vehicle primary control algorithm at which a defined response doesnot exist. Accordingly such singularity can be mathematically defined asan obstacle to be avoided by the avoidance algorithm of the presentinvention. With this approach, the avoidance algorithm will cause thevehicle to be maneuvered such that it does not reach a state where theprimary control algorithm is at a singularity.

Another implementation could be in the construction of an avoidancealgorithm to control the body orientation of a vehicle during approachto a destination so that the line of sight to the destination ismaintained to be not more than X degrees off the vehicle bore-sight(x-direction). In this instance the obstacle is not a material item buta mathematical construction. In addition, the boundary surface for thisconstruction is a cone emanating from some point relative to the vehiclealong the body x and having a cone angle of 2X. The avoidance algorithmwould be constructed using the same equations as before. However, thedefinitions of the weighting functions would be more involved.

Second Embodiment Operational Boundary Avoidance EXAMPLE 1 MinimumAltitude

Application of the method and system of the present invention toavoidance of operational boundaries (rather than obstacles) is describednext as a second preferred embodiment. As will be explained furtherbelow, the basic approach is the same as outlined for the firstembodiment.

Operational boundaries, outside of which a vehicle travels normally, canbe addressed with the method of the present invention as describedabove. For instance, as illustrated in FIG. 5, if an operationalboundary for a vehicle is a minimum altitude, a sphere can be used as ageometric boundary 310 having a center located at the center of theearth and having a radius suitable to place a surface of the sphere atthe desired minimum altitude. Accordingly, by following the approachdescribed above for avoidance of obstacles, the vehicle 10 would avoidflying below the minimum altitude boundary 310 and hence not impact thesurface 300. In this situation, an oblate spheroid represents theminimum altitude boundary 310 of the vehicle 10 and corresponds to theobstacle 300 in the prior descriptions. As will be appreciated by thoseskilled in the art, the geometric boundary 310 described in all examplesand embodiments herein is not limited to a sphere surrounding an object300 and may be a continuous regular or irregular geometry which may befluid in shape and as complicated as required.

FIG. 5 illustrates the avoidance guidance command e_(avoid) for avehicle 10 in each of positions A, B and C shown. In FIG. 5, Rrepresents the surface point of tangency on the minimum altitudeboundary surface and P represents an intercept point on the surface 300to be avoided. Vector e_(NI) represents normal vector to the surface tobe avoided at point P. The other vectors shown in FIG. 5 have the samemeaning as do the vectors by the same names shown in FIGS. 3A, 3B and 4.Equations used to solve for e_(avoid) for a vehicle 10 located atpositions A, B, and C are similar to those of eqns. (1a) through (6b)above.

EXAMPLE 2 Maximum Altitude

Another operational boundary which can be addressed by the presentinvention is a maximum altitude boundary. Like the minimum altitudeboundary situation illustrated in FIG. 5, the maximum altitude boundaryscenario illustrated in FIG. 6 uses an oblate spheroid to represent themaximum ceiling 300, which corresponds to a surface of the obstacle 300in the prior descriptions. A maximum ceiling geometric boundary 310 isdefined a predetermined distance from the maximum ceiling spheroid 300and the approach described above for the First Embodiment is used todetermine avoidance guidance commands for the present conditions.

FIG. 6 illustrates the avoidance guidance command e_(avoid) for avehicle 10 in each of positions A, B and C shown. In FIG. 6, Rrepresents the surface point of tangency on the maximum altitudeboundary 310 and P represents an intercept point on the surface 300 tobe avoided. Vector e_(NI) represents normal vector to the surface 300 tobe avoided point P. The other vectors shown in FIG. 6 have the samemeaning as the vectors by the same names shown in FIGS. 3A, 3B and 4.Equations used to solve for e_(avoid) for a vehicle 10 at positions A, Band C are similar to those of eqns. (1a) through (6b) above.

EXAMPLE 3 Use for Navigation of a Submerged Vehicle

While the geometric boundary surfaces have been illustrated thus far assurfaces of simple geometry, and while the exemplary vehicles describedthus far have been aircraft or missiles, the method of the presentinvention is equally applicable to other types of implementations. Forinstance, the method can be applied to the navigation of a submarinewhile traveling submerged and can be applied to boats and shippingvessels as well.

FIG. 7 illustrates an irregularly shaped geometric boundary surface 310as may be applied in an application for obstacle avoidance for asubmarine 10 application. The actual terrain to be avoided can besimulated by a smoothed surface 300 to simplify the topography of thegeometric boundary surface 310. Because the relevant guidance vectorsand determination of avoidance guidance command e_(avoid) are the sameas illustrated and described in regard to the prior examples, FIG. 7omits these details for brevity and clarity. However, equations used tosolve for e_(avoid) in this application are similar to those of eqns.(1a) through (6b) above.

EXAMPLE 4 Avoidance of Moving Obstacles

While the above example situations illustrate obstacles which a vehiclemust avoid, the present method is equally applicable to avoidance ofactive threats or moving obstacles. In the case of an approachingthreat, if the position of the threat is known, a geometric boundary canbe defined around the threat position and the geometric boundaryposition of the threat can be continually updated. A vehicle which isbeing guided in accordance with the present method will thusautomatically be directed to avoid the boundary and thus the threatposition even as the obstacle or threat moves in relation to the movingvehicle.

Avoidance of a threat is illustrated in FIG. 8 showing a moving vehicle10 having a geometric boundary 310 moving with the vehicle 10, and amoving obstacle to be avoided, a missile 300. As illustrated in FIG. 8,the avoidance commands change, due to use of the present method, basedon the relative position between the moving vehicle 10 and the movingthreat missile 300. Specifically at threat positions t1, t2, t3, t4, t5and t6, the corresponding avoidance guidance commands are shown as a1,a2, a3, a4, a5 and a6. In the case corresponding to threat position t1,where the missile 300 is outside the geometric boundary 310, thecommands are blended between the desired flight path e_(desired) and thetangent based avoidance path e′_(tan). In the case corresponding tothreat positions t2-t4, where the missile is outside the geometricboundary but closer than position t1, the weighting of the tangent basedavoidance direction is increased. In the case corresponding to threatposition t5, where the missile is at the surface point of tangency, R,the avoidance command a5 is based on the geometric boundary tangentavoidance command e′_(tan). In the case corresponding to threat positiont6, where the missile is inside of the geometric boundary 310, theavoidance command is weighted between an anti-intercept command and atangent based command as described in eqns. (5a) and (5b) above for theFirst Embodiment. Thus, e′_(tan) and the functions used to calculate itmust be reevaluated at each point along the flight path to continuallyupdate the steering command, e_(avoid). Because the relevant guidancevectors and determination of avoidance guidance command e_(avoid) arethe same as illustrated and described in regard to the prior examples,FIG. 8 omits these details for brevity and clarity. However, equationsused to solve for e_(avoid) in this application are similar to those ofeqns. (1a) through (6b) above.

EXAMPLE 5 Avoiding Moving Object Launched from Moving Vehicle

As noted in the background of the invention, one shortcoming with theconventional avoidance technique based on use of geometric boundaryshapes occurs in the situation of launching a missile from an aircraft.Specifically, because the conventional boundary shape method requiresthat the geometric shape be made very small so that the vehicle and themissile are not both inside the geometric shape, the resulting geometricshape would be smaller than the vehicle to be avoided and the missilewould thus be able to fly into the launching aircraft. As illustrated inFIG. 9A, at the time of launch, a missile 410 and launching aircraft420, both within the geometric shape 310, have the same velocity anddirection vector. The desired missile guidance command, however, wouldpotentially cause the missile 410 to pull up upon launch and strike theaircraft 420.

In contrast, the method according to present invention, is able toprovide avoidance guidance commands which will cause a launched missile410 to avoid the aircraft 420 from which it is launched. As illustratedin FIG. 9B, a launched missile 410 would first maneuver outside of thegeometric boundary 310 which surrounds the aircraft 420 and then turnonto the desired guidance course. More specifically, FIG. 9B shows themissile guidance states at four points during the launch and flightsequence, P1, P2, P3, and P4. At positions P1 and P2, which correspondto the situation where the missile 410 is inside the geometric boundary310, the boundary tangent vector and a vector having a directionopposite to the center of the aircraft are blended together to obtainthe avoidance command as described above with reference to the FirstEmbodiment. (In this example, the center of the aircraft is defined as areference point on the aircraft that represents the whole aircraft). Atposition P3, which corresponds to the situation where the missile 410 isat the surface of the geometric boundary 310, the avoidance command isbased on the geometric boundary tangent avoidance vector. At positionP4, which corresponds to the situation where the missile 410 is outsideof the geometric boundary 310, the avoidance command is obtained byblending together the geometric boundary tangent command e′_(tan), andthe desired missile guidance command, precisely as shown and illustratedin FIG. 3A and as described in the text accompanying FIG. 3A. Thus,e′_(tan) and the functions used to calculate it must be reevaluated ateach point along the flight path to continually update the steeringcommand, e_(avoid). Because the relevant guidance vectors anddetermination of avoidance guidance command e_(avoid) are the same asillustrated and described in regard to the prior examples, FIG. 9B omitsthese details for brevity and clarity. However, equations used to solvefor e_(avoid) in this application are similar to those of eqns. (1a)through (6b) above.

Third Embodiment Control of Vehicles in Formation

The method by which the present invention may be used for control ofvehicles traveling in formation is described next as a third preferredembodiment. This embodiment builds on previous embodiments with theaddition of sink elements for position, velocity and other statecontrol.

As previously noted, an object to be avoided, and/or the surroundingregion, can be referred to as a source element, while objects or regionstowards which the vehicle is attracted can be referred to as sinkelements.

EXAMPLE 1 Geometric Boundary Formed from Source and Sink Elements

As indicated above, the method of the present invention may be used tomaintain control of vehicles in formation and to maintain the positionand separation of the vehicles in that formation. One approach inaccordance with the present method is to replace the source elementsinside of a geometric boundary with sink elements in combination withpreviously mentioned avoidance boundaries. Because a vehicle isattracted to sink elements, the sink elements will thus guide thevehicle to a particular location or guide the vehicle to a particularspeed and location, and keep the vehicle from colliding with othervehicles in the formation.

FIGS. 10A and 10B illustrate the situation where two aircraft 420A and420B are operating in a formation. In one approach, lead aircraft 420Aprovides data to trailing aircraft 420B which defines for trailingaircraft 420B acceptable positions of formation flight. This provideddata defines for trailing aircraft 420B geometric sink elements 520.FIG. 10A is a side view of the aircraft 420A and 420B in formation. Alsoshown are boundaries 510 around each aircraft 420A and 420B,respectively, which is the limit of extent of the source potential fieldwhich can be defined. Boundary 540 represents a limit of the extent ofthe source tangent field. Boundary 530 represents a limit of the extentof the sink potentials, and the sink potentials have no geometric basedavoidance vector outside of this boundary. Inside this boundary, thestrength of sink 520 is defined by functions k₁(x) and k₂(x), which areconstructed similar to f₂(x) and f′₂(x), where the definition of L isnow a surface. Thus sink 520 will only attract objects which are withinthe limit of boundary 530. The boundaries 510 will operate as geometricboundaries 310 described in the First and Second Embodiments, therebypreventing collision of the aircraft 420A, 420B while sinks 520 guidethe aircraft 420A, 420B to predefined relative positions.

The mechanism by which the effects of sinks 520 are added to the spatialavoidance guidance equations is as follows: Inside the boundary 530, thevector e_(attract-sink) is constructed such that

E _(attract-sink) =k ₁(x)e _(sinkpnt) +k ₂(x)e _(desired)  (7a)

where e_(sinkpnt) is a vector in the direction from the vehicle to acenter of a sink; where 0≦k₁(x)≦∞ and 0≦k₂(x)≦∞, such that k₁(x) andk₂(x) are not both zero; and where k₁(x)=0, when the vehicle 10 iscoincident with surface L (not shown), and k₂(x)=0, when the vehicle 10is coincident with the sink 520. Both k₁(x) and k₂(x) are linear ornon-linear continuous functions with x being a parameterized constant,function, or family of functions as described above. As above, functionsk₁(x) and k₂(x) should be constructed such that ∥E_(attract sink)∥≠0.

The vector e_(attract-sink) is calculated in equation 7b based onE_(attract-sink) computed in equation 7a:

e _(attract-sink) =E _(attract-sink) /∥E _(attract-sink)∥  (7b)

As will be appreciated by those skilled in the art, the e_(attract-sink)directional vector is then mixed with the e_(avoid) command, per eqns.8a and 8b below, to get e_(avoid-final), thus preventing collisions.

e _(avoid-final) =E _(avoid-final) /∥E _(avoid-final)∥  (8a)

E _(avoid-final) =z ₁(x)e _(desired) +z ₂(x)e _(tan)  (8b)

where 0≦z₁(x)≦∞ and 0≦z₂(x)≦∞, such that z₁(x) and z₂(x) are not bothzero; and where z₁(x)=0, when the vehicle 10 is coincident with surfaceL (not shown), and z₂(x)=0, when the vehicle 10 is coincident with thesink 520. Both z₁(x) and z₂(x) are linear or non-linear continuousfunctions with x being a parameterized constant, function, or family offunctions as described above. As above, functions z₁(x) and z₂(x) shouldbe constructed such that ∥E_(avoid-final)∥≠0.

The e_(attract-sink) directional vector then becomes the e_(desired)vector of the first two examples. Hence, as the following vehicleapproaches the leading vehicle, it will be directed toward the sinklocations and away from the leading vehicle. As the following vehiclebegins to slow or recede, depending on the definition of the transitionzone, it will begin to ignore the avoidance commands and be directedtoward the sink locations. The transition zone definition becomesimportant in this discussion.

For formation maneuvering, such as flying, the transition zone can bedynamically varied and/or its influence dynamically increased ordecreased over time, space, or based on the condition of the vehicles,to more specifically tailor the effect on the vehicles. Specifically,for instance, in the situation where aircraft are joining a formation,the transition zone can be brought into existence only when certaincondition(s) are satisfied, such as when the rate of closure between thevehicles is below a specified threshold. Similarly, the transition zonecan be removed when one aircraft begins to recede from another. Withsuch an approach, if an aircraft is approaching another aircraft at toogreat a speed, the transition zone, and hence the effect of theattracting sink, does not influence the guidance of the vehicle, butinstead the avoidance algorithm, unaffected by the sink, will commandthe vehicle to maneuver so as to avoid a collision. However, once theavoidance algorithm-based control commands have moved the vehicles intoa safe separation distance, the transition zone will be created and thee_(attract-sink) vector-based effect will head the vehicle towards thedesired position for formation flying. Therefore, even if the vehiclesare approaching each other with too much speed, the avoidance algorithmwill cause them to avoid each other. When the vehicles start to moveapart and have reached a safe distance, the e_(attract-sink) vector willonce again be the desired direction toward which the vehicle will fly.

Note the implementation described above, and shown in FIGS. 10A and 10B,differs from the previously described embodiments in that no geometricbased avoidance commands are generated by the sink elements.Specifically, the geometric-based commands developed through theaircraft avoidance, using 420 and 510, are blended with thenon-geometric-based attracting commands to define the final formationflying guidance command.

FIG. 10A also shows the formation avoidance commands which are generatedfor aircraft 420B at each of six relative positions between aircraft420A and 420B. Specifically, FIG. 10A shows six arrows representing theformation avoidance command that aircraft 420B would receive if aircraft420B were located at the corresponding formation position f1, f2, f3,f4, f5 and f6, respectively. As trailing aircraft 420B approachesleading aircraft 420A, the corresponding avoidance commands steeraircraft 420B to the sink 520. If aircraft 420B closely approachesaircraft 420A so that it comes within the range of the source field 510surrounding aircraft 420A, then aircraft 420B would receive a furtherguidance command so as to avoid a collision.

Aircraft 420A and 420B can break formation either by overriding theabove described guidance method, or can break formation while stillunder such guidance method by turning off the sinks 520. In the absenceof the attracting force from the sinks 520, the aircraft 420A and 420Bwould separate.

It will be appreciated that the size and location of the boundaries andthe sinks must be tailored to achieve the desired spacing and relativepositions of the vehicles in a given formation. Because the presentinvention is not limited to a fixed size or shape boundary, the size andshape of the boundaries can be adjusted to achieve the objectives of aparticular situation. For instance, increasing the geometric boundarysize results in avoidance commands which are able to act at a furtherdistance from a vehicle. On the other hand, decreasing the boundary sizewill allow a second vehicle (or threat) to move closer to a firstvehicle before avoidance guidance commands would be generated.

Moreover, for formation flying, the size of the boundaries may bedynamically altered based on the relative velocity between the vehicles.For instance, when the relative velocity between vehicles is nearlyzero, the size of the boundaries may be reduced to a minimum safe size.However, when the relative velocity between the vehicles is large, thesize of the boundaries can be increased to allow collision avoidanceprotection within the maneuverability limits of the vehicles.

FIGS. 10A and 10B also show that a given situation can include multiplesinks 520 and/or sources 510 within a single boundary 530. The presentinvention can be applied with an arbitrary number of sink potentialelements each representing individual points, or many sink potentialsmay collectively define a sink surface or curve.

EXAMPLE 2 Dual Zone Combined Source and Sink Geometric Boundary

Another possible method for obstacle avoidance when flying in aformation is shown in FIG. 10C. A multi-zone method having a sourcegeometric boundary 510 emanates from a point and surrounds vehicle 420Athereby defining a first zone 510. A sink geometric boundary 520emanates from the same point on vehicle 420A and surrounds sourcegeometric boundary 510 thereby creating a second zone or interactiveregion 525 between the source geometric boundary 510 and the sinkgeometric boundary 520. Within the source geometric boundary 510, theavoidance vector is computed as shown in the First Embodiment. Beyondthe sink geometric boundary 520, the avoidance vector is computed asdisclosed above in the Second Embodiment. Within the interactive region525 between source geometric boundary 510 and sink geometric boundary520, the avoidance vector is computed as a combination of the repellingforce of the source geometric boundary and the attracting force of thesink potential of the sink geometric boundary. A resulting relativeavoidance vector may thereby be computed for each point within theinteractive region 525.

The spatial guidance method of Example 2 above operates such that whenvehicle 420B is flying outside the dual geometric boundaries 510, 520,vehicle 420B is attracted to sink boundary 520 and the avoidance vectoris directed toward vehicle 420A. When vehicle 420B is within theinteractive region 525, the relative avoidance vector directs vehicle420B to maintain a position within that region 525 based on weightedfunctions. As vehicle 420B approaches source geometric boundary 510, therepelling component of the relative avoidance vector is increased orweighted in favor of the direction away from vehicle 420A, therebyeffectively directing vehicle 420B away from vehicle 420A. As vehicle420B moves away from source geometric boundary 510, the attractingcomponent of the relative avoidance vector is increased or weighted infavor of the direction toward vehicle 420A, thereby effectivelydirecting vehicle 420B toward vehicle 420A.

The benefit of this approach is that solutions need be computed for onlyone source and one sink. Further, this approach allows one vehicle information to move independently of another in all directions within theinteractive region surrounding one of the vehicles. Further, dependingupon the requirements, the boundaries of the source and sink may beformed in any size and shape. Equations for calculating the avoidancevector within the interactive region are similar to equations (1a)through (8B) above. As one of ordinary skill in the art will appreciate,the equations above would be applied in the same manner, adjusted forthe particular geometry of the embodiment.

In a modification of this approach shown in FIG. 10D, all vehicles information could have dual source and sink geometric boundaries, therebyproviding a region surrounding each vehicle for formation flight andaffording more complete vector avoidance control during formationflight. As shown, a sink overlap region 526 forms between sink geometricboundaries of vehicles flying within a relatively close proximity to aboundary region of another vehicle. By this method, it is possible tocalculate the appropriate steering vector for a plurality of vehiclesand to maintain a desired separation distance between them duringformation flight. As one of ordinary skill in the art will appreciate,the equations above would be applied in the same manner, adjusted forthe particular geometry of the embodiment. Equations for calculating theinterrelated avoidance vectors within the interrelated interactiveregions of multiple vehicles are significantly more complicated thanwith the simple prior examples. However, these equations arefunctionally similar.

Exemplary Field Diagrams Generated by Using the Method for MultipleSource and Sink Conditions

The method of the present invention and the results of the applicationof the present invention are further explained by vector field diagramsin FIGS. 11-19. These vector field diagrams are simulation results fromthe application of the method of the present invention as applied toseveral situations comprising stationary impediments and/or stationaryvehicles in a two dimensional space. The reason for applying the presentinvention to a two dimensional space and to stationary impedimentsand/or stationary vehicles in the following discussion is for simplicityof explanation only. Those of skill in the present art will appreciatethat the present invention can be applied with equal ease for dimensionsabove two and for both moving impediments and vehicles, however, staticfigures representing such dynamic situations are more difficult torender. Additionally, while the geometric boundaries shown in FIGS.11-19 consist of circles and planes, this is not a limitation of thepresent invention. In practice, the boundaries can be represented by anysmooth function.

FIGS. 11-19 were constructed using weighting functions of the followingform which also illustrate the intent of the general functiondescriptions of eqns. 1a though 6b discussed in the First Embodiment.However, for clarity and brevity when discussing specific examples ofthe present method, the following discussion will be limited andspecifically directed to that of the First Embodiment. It will beunderstood that the method could be similarly applied to and similarresults computed for all embodiments discussed herein.

Referring now to FIGS. 11-19, the equations used to calculate the vectordiagrams include:

E′ _(tan) =h ₁(x)e _(N) +h ₂(x)e _(tan)  (A)

where

h₁(x)=0.0, and

h₂(x)=1.0.

Further, equations include:

E _(avoid) =f ₁(x)e _(desired) +f ₂(x)e′ _(tan)  (B)

where when e_(desired)·e_(r0)<0,

f₁(x)=x where r_(dist)/r₀, and

f₂(x)=−x*(e_(desired)·e_(tan))+sqrt(1−x²*(1−(e_(desired)·e_(tan))²))

where r_(dist) is the distance from the current point to the nearestpoint on the obstacle, and r₀ is the distance from the current point tothe obstacle definition point.

A result of the above f₁(x) and f₂(x) construction is that E_(avoid) isnow a unit vector having direction and unit magnitude and hence:

e_(avoid)=E_(avoid).

Further, as computed according to the equations of the First Embodiment:

E* _(tan) =g ₁(x,γ)e _(desired) +g ₂(x,γ)e′ _(tan),  (C)

where the functions g₁(x, γ) and g₂(x, γ) were implemented as shown inthe following. In the equations below, two principle situations must beconsidered:

(A) where (e_(desired)·e_(r0))<0 and

(B) where (−e_(rt0)·e_(r0))≦0. In the following equations:

x is defined above as (x=r_(dist)/r₀)

γ is a boundary defined by the following conditional statements.

In the first situation (A) where, (e_(desired)·e_(r0))<0, two sub-casesmust be considered:

(1) When: (−e_(rt0)·e_(r0))>0 and (−e_(rt0)·e_(desired))>0 then$\begin{matrix}{{{g_{2}\left( {x,\mathrm{\Upsilon}} \right)} = \left( {1 + {\left( {e_{desired} \cdot e_{r0}} \right)\left( {{- e_{rt0}} \cdot e_{r0}} \right)\left( {{- e_{rt0}} \cdot e_{desired}} \right)}} \right)},{with}} \\{{g_{1}\left( {x,\mathrm{\Upsilon}} \right)} = {{{- {g_{2}\left( {x,\mathrm{\Upsilon}} \right)}}*\left( {e_{\tan}^{\prime} \cdot e_{desired}} \right)} + {{sqrt}\left( {1 - {{g_{2}\left( {x,\mathrm{\Upsilon}} \right)}^{2}*}} \right.}}} \\{{\left. \left( {1 - \left( {e_{\tan}^{\prime} \cdot e_{desired}} \right)^{2}} \right) \right)},{{{when}\quad {e_{\tan}^{\prime} \cdot e_{desired}}} > 0},{and}} \\{{{g_{1}\left( {x,\mathrm{\Upsilon}} \right)} = {{sqrt}\left( {1 - {g_{2}\left( {x,y} \right)}^{2}} \right)}},}\end{matrix}$

 when e′_(tan)*e_(desired)≦0.

(2) Otherwise, when (−e_(rt0)·e_(r0))≦0 or when (−e_(rt0)·e_(desired))≦0

g₁(x, γ)=0.0

g₂(x, γ)=1.0.

In the second situation (B) where, (e_(desired)·e_(r0))<0, two casesmust be considered:

(1) when: (e_(desired)·e_(r0))<1

g₂(x, γ)=(1−(e_(desired)·e_(r0)))*x

g₁(x, γ)=−g₂(x, γ)*(e′_(tan)·e_(desired))+sqrt (1−g₂(x,γ)²*(1−(e′_(tan)·e_(desired))²)).

(2) otherwise when: (e_(desired)·e_(tan))≧1

g₁(x, γ)=1.0

g₂(x, γ)=0.0.

In each of the above, e_(r0) is the unit vector from the current pointto the obstacle definition point, e_(rt0) is the unit vector from thedestination to the nearest point on the obstacle, and where x is acontinuous function. The symbol “·” represents the vector dot productfunction.

The above example illustrates the types of blending functions used inthe development of the examples, and does not represent specificalgorithms required to implement the method of the invention.

As shown in FIGS. 11-19, each point in the field represents a possiblelocation of a vehicle, and the small line coming from each pointindicates the direction of the avoidance guidance command (e_(avoid))which the vehicle would receive if located at that point. In each ofFIGS. 11-19, the nominal desired guidance command from any one of thepoints in the vector field is a straight line from that point to thedesired destination.

Specifically, FIG. 11 represents a situation where only one obstacle 300is present. As noted above, the nominal desired guidance command 611 isa vector originating from any one of the points 612 in the vector fieldis a straight line in a direction from that point 612 to the desireddestination 610. As can be seen in FIG. 11, the direction of theguidance command 611, as indicated by the direction of the small lineswhich radiate from the location points 612, is a function of thelocation of the point within the field relative to both the obstacle 610and the destination. For points 612 which are far from the obstacle 300,such as points 612 in the upper right hand corner of the field, thedirection of the guidance command 611 is nearly straight towards thedestination 610 and is essentially unaffected by the obstacle 300.

In contrast, points 612 which are behind the obstacle 300, relative tothe destination 610, indicate a direction of the guidance command 611which will steer a vehicle (not shown) smoothly around the obstacle 300.It should be noted the degree to which the guidance command 611 differsfrom the nominal desired direction varies as a function of the distancefrom the point 612 to the boundary 620. This is because the guidancecommand 611 is determined by blending together the nominal desireddirection and e_(int) in varying proportion depending on distance fromthe obstacle 300.

It should also be noted that the direction of the guidance command 611for each point smoothly transitions from the interior of boundary 620 tothe exterior of boundary 620. Thus, vehicles can be smoothly controlledeven in the case where obstacle geometric boundaries 620 are crossed.

FIG. 12 represents a situation where multiple obstacles are present inthe vector field. While only one obstacle is shown with a referencenumber 300 in FIG. 12, each of the three black dots which are surroundedby a circle are obstacles 300, the surrounding circle of each obstacle300 is its corresponding boundary 620. In the case where more than oneobstacle 300 is present, an avoidance direction for each obstacle iscalculated for each of the positions or points 612 in the vector field.Then, a weighting function is used, for each position 612, to prioritizethe guidance commands calculated based on each of the several obstacles300. By weighting the contribution of each calculated guidance commandand blending them together to produce a single normalized guidancecommand 611, the influence of multiple obstacles 300 can besimultaneously addressed.

In weighting the calculated guidance commands, two operational zoneswere defined and separate weighting functions were defined forapplication in each zone. The first operational zone is inside aboundary and the second operational zone is outside a boundary.

In the case where the point is inside the boundary, weighting function(10a) below is used. In the case where the point is outside of theboundary, weighting function (11a) is used.

W _(I) =M/(%_(RI))  (10a)

W _(O)=1/(%_(RI) ·R ²·MAX(%_(RO) , M/(%_(RI))))  (11a)

where

(%_(RI))<1 inside the geometric boundary, and

(%_(RI))>1 outside the geometric boundary.

As can be seen from equation (10a) above, in the case of a point whichis inside the boundary, for the avoidance vector calculated from each ofthe obstacles, the avoidance vector is divided by the percent ofintrusion into the geometric boundary (%_(RI)) along e_(int) andmultiplied by M, the number of obstacles in the space. To determine thecomposite (or final) avoidance command, all of the scaled avoidancecommands are summed and the result normalized to produce the finalavoidance commands as shown by the guidance command vectors in FIG. 12.Thus, determination of the final avoidance command can be expressed asshown below in equation (10b). $\begin{matrix}{e_{avoid} = \frac{\left( {\sum{e_{{avoid}{(n)}} \cdot W_{I{(n)}}}} \right)}{{\sum{e_{{avoid}{(n)}} \cdot W_{I{(n)}}}}}} & \text{(10b)}\end{matrix}$

where n is the number 1 through M of the point being considered in theweighting function.

As can be seen from equation (11a) above, in the case of a point whichis outside the boundary, for the avoidance vector calculated from eachof the obstacles, the avoidance vector is weighted by a non-linearfunction which is a function of the percent distance outside of theboundary relative to the local geometric boundary radius (%_(RO)), thelocal geometric boundary radius R, and W_(I).

As with points inside the boundary, to determine the composite (orfinal) avoidance command, all of the scaled avoidance commands aresummed and the result normalized to produce the final avoidance command.Thus, determination of the final avoidance command can be expressed asshown below in equation (11b). $\begin{matrix}{e_{avoid} = \frac{\left( {\sum{e_{{avoid}{(n)}} \cdot W_{O{(n)}}}} \right)}{{\sum{e_{{avoid}{(n)}} \cdot W_{O{(n)}}}}}} & \text{(11b)}\end{matrix}$

It should be noted that the weighting function used to weight theavoidance commands calculated from each of the obstacles is not limitedto functions (10a) and (11a) as described above and other weightingfunctions can be used.

The guidance command vectors shown in FIGS. 13-19 were determined basedon the same weighting approach described above for FIG. 12.

FIG. 13 represents a situation where multiple obstacles are present inthe vector field and where the boundaries 620 of the obstacles overlap.While only one obstacle 300 has a reference number in FIG. 12, each ofthe three black dots which are surrounded by a circle are obstacles 300,the surrounding circle of each obstacle is its corresponding boundary620. The guidance command vectors shown in FIG. 13 were determined basedon the same weighting approach described above for FIG. 12.

FIG. 14 represents a situation which is essentially similar to thatshown in FIG. 13 but where the obstacle is modeled as a curve or surfacecomposed of an infinite number of source elements. The calculation of%_(RI) is based on the length from the boundary surface 620 to thesource surface 613 along e_(int) rather than from the boundary surface620 to the center of the impediment location 300.

FIG. 15 represents a situation which is essentially similar to thatshown in FIG. 14 but where the destination 610 is inside a geometricboundary 620. It should be noted here that the guidance command vectors611, as shown, will steer a vehicle into a geometric boundary 620 toreach the destination while avoiding the surface of the sourcepotentials 613.

FIG. 16 represents a situation which is essentially similar to thatshown in FIG. 14 but where one of the boundaries 620 is an operationalboundary 620A such as a maximum ceiling and is thus a non-closedinfinite impediment in the avoidance vector solution.

FIG. 17 represents a situation which is essentially similar to thatshown in FIG. 16 and which demonstrates an additional aspect of thepresent invention. Specifically, in the prior discussions, the tangentvector to the geometric boundary was always described as being in thedirection of the desired guidance command. In some situations such ascontrolled traffic patterns, it is desirable to use the tangent vectorto the geometric boundary in a direction which is independent from thedesired guidance command. Such a use of the tangent control isapplicable to the management of shipping lanes or other traffic patternswhere there are rules with respect to which size vehicles should beavoided. Accordingly, in FIG. 17, one of the obstacles 300B has beendefined to direct the current point in a direction clockwise around theobstacle; another of the obstacles 300A has been defined to direct thecurrent point in a direction counter-clockwise around the obstacle.

FIG. 18 represents a situation which is essentially similar to thatshown in FIG. 17 which demonstrates an additional aspect of the presentinvention. Specifically, one of the source elements has been substitutedby a sink curve element 614. As discussed above, such an element has theeffect of attracting a vehicle to its location. This type of arrangementcan be applied in logistics planning models where a collection area isdefined for accumulation of vehicles at a location. Once the vehicleshave been collected at that location, the sink element 614 could bedissipated to permit the accumulated group to progress through theremainder of the space to the final destination 610. FIG. 18 alsoillustrates aspects of formation flying as discussed with reference tothe Third Embodiment.

FIG. 19 represents a situation applicable in traffic management. In thisarrangement, sink elements 614 are defined so as to move along a definedcorridor 701 in the direction of arrow A. Because vehicles are attractedto the sinks 614, and because the sinks 614 are controlled to move alongthe defined corridor 701 in a controlled manner, the vehicles willautomatically join and follow the controlled travel of the sinks 614. Asshown in FIG. 19, the corridor 701 has boundaries 700 which tend toconfine to the corridor 701 vehicles which have not yet moved intoposition at a sink 614. Also as shown, the boundary 720 of each sink 614can be sized in accordance with the size of the corridor 701.

Fourth Embodiment 6 Degree of Freedom (6-DOF) Control where Both Vehicleand Object are within a Geometric Boundary

Use of the method of the present invention for control of vehicles in6DOF is described next as a Fourth Embodiment.

As shown in FIGS. 14-19, obstacles 300 are contained within, orpositioned behind, geometric boundaries 620. However, according to theFourth Embodiment of the present invention shown in FIGS. 20 and 21,both the vehicle to be controlled and the obstacles are contained withingeometric boundary surfaces which can be used to determine the controlrequired for vehicles and objects operating in 6-DOF.

As will be described next, in the case where both the vehicle 810 to becontrolled and the obstacles 800 to be avoided are contained withingeometric boundary surfaces 811 a, 811 b, an interaction between thesurfaces within a field of influence 813 will produce a moment about theguided vehicle. This moment can be used to extend the guidance approachand method.

More specifically, the approaches and methods described thus far forcontrolling missile systems can provide directional guidance in only 3Degrees-of-Freedom (3-DOF) (i.e., translations along the directions x,y, and z axes). However, in the situation where a source potential curveor surface 811 b is used to represent the guided vehicle 810 and asource potential curve or surface 811 a is also used to represent anobstacle 800, an extension of the guidance commands 611 to 6-DOF (i.e.,both translations along and rotations about each of the directions x, y,and z) can be achieved. Specifically, the 6-DOF method, however,requires additional state information than the previous 3-DOFembodiments. The previous embodiments do not require attitudeinformation, whereas 6-DOF control requires knowledge of the vehicle 810attitude along with other state information. Further, additionalimplementations of the 6-DOF guidance system can be generated if threat,obstacle, or target attitude information is also provided.

Specifically, the effect can be constructed by using the vector e_(int)and the distances to some or all of the points defined by vectors whichare parallel to e_(int) that intersect both the obstacle sourcepotential curve or surface 811 a and the vehicle source curve or surface811 b. At each of the intersection points on the guided vehicle sourcecurve 811 b, a weighting function is applied to average normal andaverage tangent vectors of the obstacle source potential curve 811 a.The weighting function is based on the corresponding distance to theobstacle source potential curve 811 a within the field of influence 813.Through this approach, a distributed repulsive force can be constructedalong the guided vehicle source potential curve 811 b. The distributedforce that can be applied about a point 812 within the guided vehiclesource potential curve 811 b will produce a moment 817 about that point812 through which the commands of the vehicle may be tied. In athree-dimensional case, the moments would provide the vehicle withrotational guidance commands. These rotational guidance commands can becoupled with typical translational guidance commands to provide complete6-DOF guidance commands.

Such 6-DOF control systems can benefit systems such as a missile in anend game condition where a warhead, which has an effective direction, isused for intercept of the target.

Other potential applications include ship docking and spacecraftrendezvous and docking, which require management of thrusters toaccomplish precise docking maneuvers.

FIG. 20 also illustrates the vector field as shown in FIG. 14, however,at each point the vehicle 810 now possesses an effective attitudeboundary surface (shown at one location only for demonstration). Thevehicle effective attitude boundary surface also has a center ofrotation associated with it, which is tied to the vehicle orientation,such that:

e _(vehicle) =Z(x)e _(effsurface)

where Z(x) is a real valued constant or a continuous linear ornon-linear function, and e_(effsurface) is the effective attitudeboundary surface of the vehicle at a single point on the vehicle and isa function of the vehicle rotation. The center of rotation is notlimited to the center of gravity or to the principle axes of thevehicle.

Inclusion of vehicle attitude will increase the guidance degrees offreedom from the traditional 3-DOF to 6-DOF, with 3 translational and 3rotational degrees of freedom. Achieving the rotational degrees offreedom requires that the vehicle 810 be affected by the avoidancealgorithm. This can be achieved through construction of an apparentfield between the obstacle(s) and the vehicle such that effectivemoments are placed on the guidance algorithm which in turn will producea desired vehicle attitude command (function Z(x), described in aprevious paragraph).

The desired attitude command may be produced by integrating a distancebased function over the effective vehicle surface. Construction of thefield or distance based function may be described as follows (see FIG.21):$T = {{K(x)}{\int_{L}{\int_{L^{\prime}{(r)}}{\frac{\left( {r - r_{p}} \right)\left( {e_{N\quad \nu}*e_{Nobs}} \right)}{{r - r_{obs}}}{S_{obs}}{S}\quad \nu}}}}$

where T is the resulting attitude correction vector; K(x) is a gainscaling function to regulate the magnitude of the command; r is a localpoint on the vehicle 10 effective surface Sν (surface denoted by ν);r_(obs) is a local point on the obstacle surface; r_(p) is the center ofrotation about which the vehicle orientation will be controlled; e_(Nv)is the unit vector normal to the effective surface Sν at the point r;e_(obs) is the unit vector normal to the obstacle surface S_(obs) at thepoint r_(obs); L are the limits of integration which define the part ofthe effective surface which point in the direction of the obstaclesurface S_(obs) (denoted by obs); and L′ (r) are the limits ofintegration which define the part of the obstacle surface which isvisible from the current point r.

The resulting vector T will yield attitude commands about the pointr_(p). The magnitude of the commands may be modified by the gainfunction K(x) to yield commands which will provide a stable attitudeguidance solution. The gain function K(x) may also be a spatially basedfunction, a time based function, or a function of spatial rates(velocity, acceleration, etc), whereby function K(x) changes magnitudebased on a given state. Variation of guidance gain magnitude is typicalfor guidance implementations of this type.

There are various ways to implement the function described above. TheFourth Embodiment described here addresses the use of the boundaryrepresentations to control more degrees-of-freedom than in typical priorart methods.

In addition to providing a means for 6-DOF guidance solutions, theFourth Embodiment can be expanded beyond the six degrees-of-freedomguidance to include N degrees of guidance by surrounding points orsurfaces of interest with boundaries, and by then applying the avoidancemethods described in the First and Second Embodiments along with themethod of the Fourth Embodiment.

While the various embodiments of the present invention have thus farbeen described as a vehicle control or guidance system, it will beappreciated by those of skill in the art that the method of the presentinvention can be implemented in hardware and/or software as part or allof a vehicle guidance control system.

Guidance Control Systems Implementing the Spatial Avoidance Method

Examples of vehicle guidance control systems implementing the abovedescribed spatial avoidance method are illustrated in FIGS. 22-24.

Autonomous Vehicle Guidance Processor

FIG. 22 is an example of a configuration for a guidance processing unit910, for an autonomous vehicle guidance processor. The processing unit910 of FIG. 22 includes a navigation function 912 and a guidancefunction 914, and may also include an autopilot function 916 as well asat least one processor 918 for operation of the unit. The guidanceprocessing unit 910 interfaces with various other components, includingrandom access memory 934 and read only memory 932, which can storenavigational information as well as vehicle control algorithms. In orderto control the vehicle, the guidance processing unit 910 can receiveinput from various sources including an inertial measurement system 922as well as a navigation receiver 920 which may be a global positioningreceiver (GPS) or other type of receiver for providing navigationinformation. The guidance processing unit 910 can also be connected toan uplink data receiver 924 for receiving control commands and datacommunications from the ground or other vehicles and may further beconnected to an onboard seeker such as a radar device, forward-lookinginfrared receiver (FLIR) or other type of target-locating sensor.Finally, the guidance processing unit 910 outputs commands to a vehicleattitude control system 928 and/or aerodynamic maneuvering system 930 inorder to control the vehicle based on the current vehicle position andcondition, the presence and location of any obstacles, and thenavigation objectives to be achieved.

Piloted Vehicle Automatic Guidance Control Processor

FIG. 23 is an example of a configuration of a vehicle guidance controlsystem implementing the above described spatial avoidance method whenthe vehicle is piloted but under automatic guidance control. FIG. 23illustrates the method of the invention used in systems which are knownas fly-by-wire type systems. In a fly-by-wire type system, the avoidancecommands are automatically directed to the autopilot function 916without required intervention by the pilot/user 917. The pilot/user 917acts as an input to the guidance system and does not directly fly theaircraft. All instructions from the pilot are fed through an autopilotfunction to produce the correct steering commands to the vehicle. Insome instances, the pilot/user may be the primary guidance processorwith the spatial avoidance method intervening only when necessary. Mostmodern aircraft operate with a fly-by-wire control system. The methodmay also be implemented such that it includes a feedback mechanism 919,which will show the pilot/user the change being made by the automaticsystem relative to the pilot/user input.

Pilot Controlled Guidance Control Processor

FIG. 24 is an example of a configuration of a vehicle guidance controlsystem implementing the above described spatial avoidance method whenthe vehicle is piloted and the pilot 917 is responsible for inputtingthe guidance control commands calculated by the method such as in aflight director system. In the example of FIG. 24, the pilot/user is inprimary control of the system and the guidance processor implementingthe method of the invention only provides feedback to the pilot/user asto how to avoid the obstacles. The feedback mechanism 919 may be throughan analog or digital indicator which also may be on an instrument panelor implemented in a heads-up display. The pilot/user ultimately makesthe determination whether or not to implement the guidance controlcommands.

It will be appreciated by those of skill in the art, that the controllaws for the above described system in accordance with the presentinvention can be implemented in computer software and stored in a memorydevice. Thus, a computer program encoding instructions in accordancewith the described method, as well as a computer readable memory devicewhich stores instructions executable by a computer to perform the stepsof the described method are also considered to be embodiments of thepresent invention.

Although the present invention has been fully described by way ofexamples and with reference to the accompanying drawings, it is to beunderstood that various changes and modifications will be apparent tothose skilled in the art without departing from the spirit and scope ofthe invention. Therefore, unless such changes and modifications departfrom the scope of the present invention, they should be construed asbeing included therein.

What is claimed is:
 1. A method of maneuvering a vehicle towards adestination in the presence of at least one obstacle, comprising thesteps of: defining a geometric boundary with respect to the obstacle;establishing a nominal guidance direction for said vehicle; establishinga nominal obstacle avoidance direction based on said geometric boundaryand a relative position of said vehicle with respect to said boundary;and determining a vehicle steering direction in accordance with afunction of the form: D _(STEERING) =F1*D _(GUIDANCE) +F2*D_(AVOIDANCE);  where: D_(GUIDANCE) is a vector with a direction in saidnominal guidance direction; D_(AVOIDANCE) is a vector with a directionin said nominal obstacle avoidance direction; D_(STEERING) is saidvehicle steering direction; F1 is a first continuous function; and F2 isa second continuous function.
 2. A method in accordance with claim 1,wherein said nominal obstacle avoidance direction is a geometric basedavoidance vector determined based on a defined geometric boundarysurrounding said obstacle.
 3. A method in accordance with claim 2,further comprising the step of determining a distance between saidvehicle and said obstacle.
 4. A method in accordance with claim 3,wherein said step of determining a vehicle steering direction is furtherbased on said distance.
 5. A method in accordance with claim 4, whereinsaid vehicle steering direction is further based on a distance-basedfield element.
 6. A method in accordance with claim 4, furthercomprising the step of determining a range of influence of said obstacleand wherein, in the case where said vehicle is beyond the range ofinfluence of said obstacle, said vehicle steering direction isdetermined without regard to the presence of said obstacle.
 7. A methodin accordance with claim 4, further comprising the step of determining arange of influence of said obstacle and wherein, in the case where saidvehicle is within the range of influence of said obstacle, said vehiclesteering direction is determined based on the presence of said obstacle.8. A method in accordance with claim 1, further comprising the steps of:determining whether said vehicle is outside of said geometric boundaryor inside of said geometric boundary.
 9. A method in accordance withclaim 8, wherein, in the case that said vehicle is outside of saidgeometric boundary, said nominal guidance direction for said vehicle isa direction substantially towards said destination.
 10. A method inaccordance with claim 8, wherein, in the case that said vehicle isinside of said geometric boundary, said nominal guidance direction forsaid vehicle is an anti-intercept direction between said vehicle andsaid obstacle.
 11. A method in accordance with claim 1, wherein F2 isapproximately equal to 1−F1.
 12. A method in accordance with claim 1,further comprising the step of steering said vehicle towards saidvehicle steering direction; and wherein said nominal obstacle avoidancedirection is based on a relative position of said vehicle to saidobstacle, and wherein said steps of establishing a nominal guidancedirection towards said destination, establishing a nominal obstacleavoidance direction based on said obstacle, determining a vehiclesteering direction based on said nominal guidance direction and saidobstacle avoidance direction, and steering said vehicle towards saidvehicle steering direction are repeated a plurality of times as saidvehicle travels.
 13. A method in accordance with claim 1, wherein: saidvehicle travels towards said destination in the presence of a pluralityof obstacles; said step of defining a geometric boundary with respect tosaid obstacle includes defining a geometric boundary with respect toeach of said plurality of obstacles; said step of establishing a nominalobstacle avoidance direction includes establishing a nominal obstacleavoidance direction for each of said obstacles; and said step ofdetermining a vehicle steering direction includes determining a vehiclesteering direction based on said nominal guidance direction and each ofsaid nominal obstacle avoidance directions.
 14. A method in accordancewith claim 1, further comprising the step of determining whether saidvehicle is approaching said obstacle or whether said vehicle is recedingfrom said obstacle.
 15. A method in accordance with claim 14, whereinsaid step of determining a vehicle steering direction includesdetermining a vehicle steering direction based on a result of said stepof determining whether said vehicle is approaching said obstacle orwhether said vehicle is receding from said obstacle.
 16. A method inaccordance with claim 1, further comprising the step of outputting asteering control signal effective to steer said vehicle in accordancewith said vehicle steering direction.
 17. A method in accordance withclaim 1, further comprising the step of displaying a steering command inaccordance with said vehicle steering direction.
 18. A method inaccordance with claim 1, wherein said vehicle steering direction iscontinuous across the geometric boundary.
 19. A method of maneuvering avehicle towards a destination in the presence of at least one obstacleand a distance-based field element, comprising the steps of:establishing a nominal guidance direction for said vehicle; establishinga geometric boundary with respect to each of said at least one obstacle;determining a distance between said vehicle and each of said at leastone obstacle; determining, for each of said at least one obstacle,whether said vehicle is inside of said geometric boundary or outside ofsaid geometric boundary; based on said step of determining, for each ofsaid at least one obstacle, whether said vehicle is inside of saidgeometric boundary or outside of said geometric boundary, establishing:in the case where said vehicle is outside of said geometric boundary,said nominal guidance direction for said vehicle is a directionsubstantially towards the destination, and in the case where saidvehicle is inside of said geometric boundary, said nominal guidancedirection for said vehicle is an anti-intercept direction between saidvehicle and said at least one obstacle; and determining a vehiclesteering direction based on an applicable one of said nominal guidancedirection, said distance, and the distance-based field element.
 20. Anapparatus for maneuvering a vehicle towards a destination in thepresence of at least one obstacle, comprising: a controller having amemory; a means for inputting a nominal guidance direction for saidvehicle into said memory; a computer for computing a nominal obstacleavoidance direction based on a geometric boundary with respect to saidat least one obstacle, and for computing a vehicle steering direction inaccordance with a function of the form: D _(STEERING) =F1*D _(GUIDANCE)+F2*D _(AVOIDANCE);  where: D_(GUIDANCE) is a vector with a direction insaid nominal guidance direction; D_(AVOIDANCE) is a vector with adirection in said nominal obstacle avoidance direction; D_(STEERING) issaid vehicle steering direction; F1 is a first continuous function; andF2 is a second continuous function.
 21. An apparatus of claim 20,further comprising: an autopilot for receiving said vehicle steeringdirection and steering said vehicle toward said vehicle steeringdirection.
 22. A method for maneuvering a first vehicle in the presenceof a second vehicle, comprising the steps of: establishing a geometricboundary with respect to the second vehicle; establishing a nominalguidance direction for each of said first and said second vehicles;establishing a nominal obstacle avoidance direction for said firstvehicle with respect to said second vehicle; determining a steeringdirection for said first vehicle in accordance with a function of theform: D _(STEERING) =F1*D _(GUIDANCE) +F2*D _(AVOIDANCE);  where:D_(GUIDANCE) is a vector with a direction in said nominal guidancedirection of said first vehicle; D_(AVOIDANCE) is a vector with adirection in said nominal obstacle avoidance direction; D_(STEERING) issaid vehicle steering direction; F1 is a first continuous function; andF2 is a second continuous function.
 23. A method for maneuvering a firstvehicle in relation to a second vehicle, comprising the steps of:establishing a first repulsive boundary around said second vehicle;establishing a first attractive boundary around said first repulsiveboundary, said first repulsive boundary and said first attractiveboundary defining an interactive region therebetween; computing asteering direction for said first vehicle such that when said firstvehicle is outside said first attractive boundary, said first vehicle isdirected toward said second vehicle, when said first vehicle is withinsaid first repulsive boundary, said first vehicle is directed away fromsaid second vehicle, and when said first vehicle is within saidinteractive region, said first vehicle is directed either toward or awayfrom said second vehicle based on a weighted function.
 24. A guidanceprocessor for maneuvering a vehicle towards a destination in thepresence of at least one obstacle, said obstacle having a geometricboundary defined with respect thereto, comprising: a device fordetermining a nominal guidance direction for said vehicle; a device fordetermining a nominal obstacle avoidance direction based on saidgeometric boundary and a relative position of said vehicle with respectto said geometric boundary; and; a device for determining a vehiclesteering direction in accordance with a function of the form: D_(STEERING) =F1*D _(GUIDANCE) +F2*D _(AVOIDANCE);  where: D_(GUIDANCE)is a vector with a direction in said nominal guidance direction;D_(AVOIDANCE) is a vector with a direction in said nominal obstacleavoidance direction; D_(STEERING) is said vehicle steering direction; F1is a first continuous function; and F2 is a second continuous function.25. A guidance processor in accordance with claim 24, wherein saidnominal obstacle avoidance direction is a geometric based avoidancevector determined based on a defined geometric boundary surrounding saidobstacle.
 26. A guidance processor in accordance with claim 24, furthercomprising: a device for determining whether said vehicle is outside ofsaid geometric boundary or inside of said geometric boundary.
 27. Aguidance processor in accordance with claim 26, wherein, in the casethat said vehicle is outside of said geometric boundary, said nominalguidance direction for said vehicle is a direction substantially towardssaid destination.
 28. A guidance processor in accordance with claim 26,wherein, in the case that said vehicle is inside of said geometricboundary, said nominal guidance direction for said vehicle is ananti-intercept direction between said vehicle and said obstacle.
 29. Aguidance processor in accordance with claim 24, further comprising: adevice for determining a distance between said vehicle and said at leastone obstacle.
 30. A guidance processor in accordance with claim 29,wherein said device for determining a vehicle steering direction furtherdetermines said vehicle steering direction based on said distance.
 31. Aguidance processor in accordance with claim 30, further comprising: adevice for determining a range of influence of said obstacle andwherein, in the case where said vehicle is beyond the range of influenceof said obstacle, said device for determining a vehicle steeringdirection further determines said vehicle steering direction withoutregard to the presence of said obstacle.
 32. A guidance processor inaccordance with claim 30, further comprising: a device for determining arange of influence of said obstacle and wherein, in the case where saidvehicle is within the range of influence of said obstacle, said devicefor determining a vehicle steering direction further determines saidvehicle steering direction based on the presence of said obstacle.
 33. Aguidance processor in accordance with claim 24, wherein said device fordetermining a vehicle steering direction further determines said vehiclesteering direction based on a distance-based field element.
 34. Aguidance processor in accordance with claim 24, wherein said device fordetermining a vehicle steering direction determines said steeringdirection by blending together said nominal guidance direction and saidnominal obstacle avoidance direction based on a blending ratio.
 35. Aguidance processor in accordance with claim 24, wherein F2 isapproximately equal to 1−F1.
 36. A guidance processor in accordance withclaim 24, further comprising: a device for steering said vehicle towardssaid vehicle steering direction; and wherein said device for determininga nominal guidance direction for said vehicle determines said nominalobstacle avoidance direction based on a relative position of saidvehicle to said obstacle, and wherein said device for determining anominal guidance direction for said vehicle, said device for determininga nominal obstacle avoidance direction based on said obstacle, and saiddevice for determining a vehicle steering direction each continuallyupdate its corresponding direction as said vehicle travels.
 37. Aguidance processor in accordance with claim 24, wherein: said vehicletravels towards said destination in the presence of a plurality ofobstacles; wherein said device for determining a nominal obstacleavoidance direction determines a nominal obstacle avoidance directionfor each of said obstacles; and wherein said device for determining avehicle steering direction determines a vehicle steering direction basedon said nominal guidance direction and each of said nominal obstacleavoidance directions.
 38. A guidance processor in accordance with claim24, further comprising: a device for determining whether said vehicle isapproaching said obstacle or whether said vehicle is receding from saidobstacle.
 39. A guidance processor in accordance with claim 38, whereinsaid device for determining a vehicle steering direction determines saidvehicle steering direction based on an output from said device fordetermining whether said vehicle is approaching said obstacle or whethersaid vehicle is receding from said obstacle.
 40. A guidance processor inaccordance with claim 24, further comprising: a device for outputting asteering control signal to steer said vehicle in accordance with saidvehicle steering direction.
 41. A guidance processor in accordance withclaim 24, further comprising: a device for displaying a steering commandin accordance with said vehicle steering direction.
 42. A memory devicefor use with a guidance processor for maneuvering a vehicle towards adestination in the presence of at least one obstacle, said memory devicecomprising encoded instructions executable by the guidance processorfor: establishing a geometric boundary with respect to each of the atleast one obstacle; establishing a nominal guidance direction for saidvehicle; establishing a nominal obstacle avoidance direction based onsaid geometric boundary and a relative position of said vehicle withrespect to said geometric boundary; and determining a vehicle steeringdirection in accordance with a function of the form: D _(STEERING) =F1*D_(GUIDANCE) +F2*D _(AVOIDANCE);  where: D_(GUIDANCE) is a vector with adirection in said nominal guidance direction; D_(AVOIDANCE) is a vectorwith a direction in said nominal obstacle avoidance direction;D_(STEERING) is said vehicle steering direction; F1 is a firstcontinuous function; and F2 is a second continuous function.
 43. Amemory device in accordance with claim 42, wherein said nominal obstacleavoidance direction is a geometric based avoidance vector determinedbased on a defined geometric boundary surrounding said obstacle.
 44. Amemory device in accordance with claim 43, further comprising encodedinstructions executable by the guidance processor for: receivinginformation indicative of a distance between said vehicle and saidobstacle.
 45. A memory device in accordance with claim 44, wherein saidencoded instructions for determining a vehicle steering directionfurther determine said steering direction based on said distance.
 46. Amemory device in accordance with claim 45, wherein said encodedinstructions for determining a vehicle steering direction furtherdetermine said vehicle steering direction based on a distance-basedfield element.
 47. A memory device in accordance with claim 45, furthercomprising encoded instructions executable by the guidance processorfor: determining a range of influence of said obstacle; and wherein, inthe case where said vehicle is beyond the range of influence of saidobstacle, said encoded instructions for determining a vehicle steeringdirection further determine said vehicle steering direction withoutregard to the presence of said obstacle.
 48. A memory device inaccordance with claim 45, further comprising encoded instructionsexecutable by the guidance processor for: determining a range ofinfluence of said obstacle and wherein, in the case where said vehicleis within the range of influence of said obstacle, said encodedinstructions for determining a vehicle steering direction furtherdetermine said vehicle steering direction based on the presence of saidobstacle.
 49. A memory device in accordance with claim 42, furthercomprising encoded instructions executable by the autopilot controllerfor: determining whether said vehicle is outside of said geometricboundary or inside of said geometric boundary.
 50. A memory device inaccordance with claim 49, wherein, in the case that said vehicle isoutside of said geometric boundary, said nominal guidance direction forsaid vehicle is a direction substantially towards said destination. 51.A memory device in accordance with claim 49, wherein, in the case thatsaid vehicle is inside of said geometric boundary, said nominal guidancedirection for said vehicle is an anti-intercept direction between saidvehicle and said obstacle.
 52. A memory device in accordance with claim42, wherein F2 is approximately equal to 1−F1.
 53. A memory device inaccordance with claim 42, further comprising encoded instructionsexecutable by the guidance processor for: outputting a steering signalfor steering said vehicle towards said steering direction; and whereinsaid encoded instructions for determining a nominal obstacle avoidancedirection determine said nominal obstacle avoidance direction based on arelative position of said vehicle to said obstacle, and wherein saidmemory further comprises encoded instructions to execute saidinstructions for establishing a nominal guidance direction towards saiddestination, establishing a nominal obstacle avoidance direction basedon said obstacle, determining a vehicle steering direction based on saidnominal guidance direction and said obstacle avoidance direction, andoutputting a steering signal for steering said vehicle towards saidvehicle steering direction a plurality of times as said vehicle travels.54. A memory device in accordance with claim 42, wherein: said vehicletravels towards said destination in the presence of a plurality ofobstacles; said encoded instructions for establishing a nominal obstacleavoidance direction include encoded instructions for establishing anominal obstacle avoidance direction for each of said obstacles; andsaid encoded instructions for determining a vehicle steering directionincludes encoded instructions for determining a vehicle steeringdirection based on said nominal guidance direction and each of saidnominal obstacle avoidance directions.
 55. A memory device in accordancewith claim 42, further comprising encoded instructions executable by theguidance processor for: determining whether said vehicle is approachingsaid obstacle or whether said vehicle is receding from said obstacle.56. A memory device in accordance with claim 55, wherein said encodedinstructions for determining a vehicle steering direction includesencoded instructions for determining a vehicle steering direction basedon an output of said encoded instructions for determining whether saidvehicle is approaching said obstacle or whether said vehicle is recedingfrom said obstacle.
 57. A memory device in accordance with claim 42,further comprising encoded instructions executable by the guidanceprocessor for: outputting a steering control signal effective to steersaid vehicle in accordance with said vehicle steering direction.
 58. Amemory device in accordance with claim 42, further comprising encodedinstructions executable by the guidance processor for: outputting asteering display command in accordance with said vehicle steeringdirection for indicating a direction for steering said vehicle. 59.Computer executable software for use with a guidance processor formaneuvering a vehicle towards a destination in the presence of at leastone obstacle, said obstacle having a geometric boundary defined withrespect thereto, comprising: means for determining a nominal guidancedirection for said vehicle; means for determining a nominal obstacleavoidance direction based on said geometric boundary and a relativeposition of said vehicle with respect to said geometric boundary; andmeans for determining a vehicle steering direction in accordance with afunction of the form: D _(STEERING) =F1*D _(GUIDANCE) +F2*D_(AVOIDANCE);  where: D_(GUIDANCE) is a vector with a direction in saidnominal guidance direction; D_(AVOIDANCE) is a vector with a directionin said nominal obstacle avoidance direction; D_(STEERING) is saidvehicle steering direction; F1 is a first continuous function; and F2 isa second continuous function.
 60. An autopilot controller formaneuvering a vehicle towards a destination in the presence of at leastone obstacle, said obstacle having a geometric boundary defined withrespect thereto, comprising: a device for determining a nominal guidancedirection for said vehicle; a device for determining a nominal obstacleavoidance direction based on said geometric boundary and a relativeposition of said vehicle with respect to said geometric boundary; and adevice for determining a vehicle steering direction in accordance with afunction of the form: D _(STEERING) =F1*D _(GUIDANCE) +F2*D_(AVOIDANCE);  where: D_(GUIDANCE) is a vector with a direction in saidnominal guidance direction; D_(AVOIDANCE) is a vector with a directionin said nominal obstacle avoidance direction; D_(STEERING) is saidvehicle steering direction; F1 is a first continuous function; and F2 isa second continuous function.
 61. An autopilot controller in accordancewith claim 60, wherein said nominal obstacle avoidance direction is ageometric based avoidance vector determined based on a defined geometricboundary surrounding said obstacle.
 62. An autopilot controller inaccordance with claim 61, wherein F2 is approximately equal to 1−F1. 63.An autopilot controller in accordance with claim 61, further comprising:a device for steering said vehicle towards said vehicle steeringdirection; and wherein said device for determining a nominal guidancedirection for said vehicle determines said nominal obstacle avoidancedirection based on a relative position of said vehicle to said obstacle,and wherein device for determining a nominal guidance direction for saidvehicle, said device for determining a nominal obstacle avoidancedirection based on said obstacle, and said device for determining avehicle steering direction each continually updates its correspondingdirection as said vehicle travels.
 64. A method of maneuvering a vehicletowards a destination in the presence of at least one operationalboundary, comprising the steps of: defining a geometric boundary withrespect to the operational boundary; establishing a nominal guidancedirection for said vehicle; establishing a nominal obstacle avoidancedirection based on said geometric boundary and a relative position ofsaid vehicle with respect to said boundary; and; determining a vehiclesteering direction in accordance with a function of the form: D_(STEERING) =F1*D _(GUIDANCE) +F2*D _(AVOIDANCE);  where: D_(GUIDANCE)is a vector with a direction in said nominal guidance direction;D_(AVOIDANCE) is a vector with a direction in said nominal obstacleavoidance direction; D_(STEERING) is said vehicle steering direction; F1is a first continuous function; and F2 is a second continuous function.65. A method in accordance with claim 64, wherein the operationalboundary is selected from the group consisting of a maximum altitude, aminimum altitude, and a maximum submersion depth.